Some of the control systems used in autonomous vehicles aims to move the vehicles through the surroundings, by predicting safe motions for the vehicle avoiding obstacles. The surroundings, such as road edges, pedestrians, and other vehicles, are sensed by the sensors of the vehicle and/or at least partially known by a priori given information. However, the sensor information is noisy and uncertain and the control of the vehicle should account for such an uncertainty. Moreover, it is also can be important to optimize the operation of the vehicle associated with the predicted motion.
For example, if an autonomous vehicle travels along a road segment, the control system of the autonomous vehicle can generate a motion that minimizes one or combinations of deviations from the middle of the lane and speed variations to provide a smooth ride for the passenger and to provide a smooth traffic flow. Another example is autonomous parking, where the number of steering and forward/reverse changes could be minimized for increased efficiency and comfort of the passenger.
One objective of the motion planning is to generate an optimal path and corresponding trajectory from initial vehicle state to a target state. The states include dynamics of the vehicle, such as locations, velocities and headings of the vehicles. However, many types of vehicles, such as cars, trucks, and other nonholonomic mobile robots, are underactuated systems having nonlinear, complex, and potentially unstable dynamics. The underactuated system means that the vehicles have restrictions on their movements. Such a restriction can result from a fewer number of actuators than degrees of freedom in the system. For example, the car cannot move sideways. In addition, instability for these types of vehicles means that a small change in control input can lead to significantly different behavior (output) in the system response, where system response could include entities such as positions, velocities, and accelerations.
As a consequence, predicting safe motions that the vehicle can move along and providing reliable performance become important, but tedious tasks. To that end, a number of systems determine first the optimal path for the vehicle, and then determine the motion of the vehicle moving along the optimal path.
For example, one method uses a closed loop rapidly-exploring random tree (CL-RRT), which, instead of sampling in the control space, randomly samples the reference space, i.e., the space of locations, to grow a tree of possible locations. The states of the vehicles at newly sampled locations are determined from the closest location in the tree. However, this method does not determine the optimal motion of the vehicle.